/*
 * Compensation.c
 *
 *  Created on: Oct 4, 2024
 *      Author: Grasping
 */

/*======================================Compensation function======================================*/
#include <My_INC/PHY/PHY_math.h>

/*================================================================�ֽ���======================================================================*/

/**************�������Ǻ�������****************/
#if (SIN_COS_MODE == 0U)
#if Q31_MODE == 0
//
// Created by czt on 2023/1/30.
//
// �ο���
// https://github.com/samhocevar/lolremez/wiki/Tutorial-4-of-5%3A-fixing-lower-order-parameters
// ����еĽ���ֵ

// ʹ��lolremez�㷨���ɵĶ���ʽ���ƺ��������ڼ���sin(sqrt(x))�Ľ���ֵ
// �ö���ʽ�����������Ϊ1.455468e-9
static inline float f1(float x);
static inline float f2(float x);

static inline float f1(float x)
{
    float u = 1.3528548e-10f; // ����ʽϵ��
    u = u * x + -2.4703144e-08f;
    u = u * x + 2.7532926e-06f;
    u = u * x + -0.00019840381f;
    u = u * x + 0.0083333179f;
    return u * x + -0.16666666f; // ���ض���ʽ������
}

// ʹ��lolremez�㷨���ɵĶ���ʽ���ƺ��������ڼ���cos(sqrt(x))�Ľ���ֵ
// �ö���ʽ�����������Ϊ1.1846383e-8
static inline float f2(float x)
{
    float u = 1.7290616e-09f; // ����ʽϵ��
    u = u * x + -2.7093486e-07f;
    u = u * x + 2.4771643e-05f;
    u = u * x + -0.0013887906f;
    u = u * x + 0.041666519f;
    return u * x + -0.49999991f; // ���ض���ʽ������
}

// ���ټ���sin(x)�ĺ���
float fast_sin(float x)
{
    // ����x / �е��������֣����ڽ�xӳ�䵽[-��, ��]����
    int si = (int)(x * 0.31830988f); // 0.31830988f �� 1/��

    x = x - (float)si * _PI; // ��xӳ�䵽[-��, ��]����

    // ���si����������Ҫ��һ������x��ֵ
    if (si & 1)
    {
        x = x > 0.0f ? x - _PI : x + _PI;
    }
    float x_2 = x * x;
    // ʹ�ö���ʽ���Ƽ���sin(x)
    return x + x_2 * x * f1(x_2);
}

// ���ټ���cos(x)�ĺ���
float fast_cos(float x)
{
    // ����x / �е��������֣����ڽ�xӳ�䵽[-��, ��]����
    int si = (int)(x * 0.31830988f); // 0.31830988f �� 1/��
    x = x - (float)si * _PI;         // ��xӳ�䵽[-��, ��]����

    // ���si����������Ҫ��һ������x��ֵ
    if (si & 1)
    {
        x = x > 0.0f ? x - _PI : x + _PI;
    }
    float x_2 = x * x;
    // ʹ�ö���ʽ���Ƽ���cos(x)
    return 1.0f + x_2 * f2(x_2);
}

// ���ټ���sin(x)��cos(x)�ĺ��������ͨ��ָ�뷵��
void fast_sin_cos(float x, float *sin_x, float *cos_x)
{
    // ����x / �е��������֣����ڽ�xӳ�䵽[-��, ��]����
    int si = (int)(x * 0.31830988f); // 0.31830988f �� 1/��
    x = x - (float)si * _PI;         // ��xӳ�䵽[-��, ��]����

    // ���si����������Ҫ��һ������x��ֵ
    if (si & 1)
    {
        x = x > 0.0f ? x - _PI : x + _PI;
    }
    float x_2 = x * x;
    // ʹ�ö���ʽ���Ƽ���sin(x)��cos(x)
    *sin_x = x + x_2 * x * f1(x_2);
    *cos_x = 1.0f + x_2 * f2(x_2);
}

// ֱ��չ��ѭ��
void fast_sin_three_phase(float wt, float results[3])
{
    float phases[3] = {wt, wt - _2PI_D3, wt + _2PI_D3};
    // ������һ����λ��
    float x = phases[0];
    int   si = (int)(x * 0.31830988f);
    x -= (float)si * _PI;
    if (si & 1)
        x = (x > 0.0f) ? x - _PI : x + _PI;
    float x_2 = x * x;
    results[0] = x + x_2 * x * f1(x_2);

    // �����ڶ�����λ��
    x = phases[1];
    si = (int)(x * 0.31830988f);
    x -= (float)si * _PI;
    if (si & 1)
        x = (x > 0.0f) ? x - _PI : x + _PI;
    x_2 = x * x;
    results[1] = x + x_2 * x * f1(x_2);

    // ������������λ��
    x = phases[2];
    si = (int)(x * 0.31830988f);
    x -= (float)si * _PI;
    if (si & 1)
        x = (x > 0.0f) ? x - _PI : x + _PI;
    x_2 = x * x;
    results[2] = x + x_2 * x * f1(x_2);
}

#endif
#endif

/*==================================��ʼ������������ұ�===================================*/
/*
 * ���ƣ����ұ���ʼ������
 * ���ܣ����ұ���ʼ���������ʵ��
 * ���������
 * 		float * Sine_Data����Ҫ��ʼ��������
 * 		uint16_t Size	������ߴ�,׼���򼸸���
 * ���ز�������
 */
void Sin_Data_init(float *Sin_Data, uint32_t size)
{
    uint16_t i = 0;
    float    temp = 2.0f * _PI / size; // ���ǶȻ���Ϊsize��
    for (i = 0; i < size; i++)
    {
        Sin_Data[i] = fast_sin(i * temp);
    }
}
void Cos_Data_init(float *Cos_Data, uint32_t size)
{
    uint16_t i = 0;
    float    temp = 2.0f * _PI / size; // ���ǶȻ���Ϊsize��
    for (i = 0; i < size; i++)
    {
        Cos_Data[i] = fast_cos(i * temp);
    }
}
void SinCos_Data_init(float *Cos_Data, float *Sin_Data, uint32_t size)
{
    uint16_t i = 0;
    float    temp = 2.0f * _PI / size; // ���ǶȻ���Ϊsize��
    for (i = 0; i < size; i++)
        fast_sin_cos(i * temp, &Sin_Data[i], &Cos_Data[i]);
}
// ���ߣ���ͤ�ŵ��;���
// ���ӣ�https://zhuanlan.zhihu.com/p/12844060351
// ��Դ��֪��
// ����Ȩ���������С���ҵת������ϵ���߻����Ȩ������ҵת����ע��������
#if (SIN_COS_MODE == 1U)
/*******������ұ�********/
#if (SIN_TABLE_SIZE == 512)
static const float sin_table[SIN_TABLE_SIZE] = {
    0.000000f,  0.012272f,  0.024541f,  0.036807f,  0.049068f,  0.061321f,  0.073565f,  0.085797f,  0.098017f,
    0.110222f,  0.122411f,  0.134581f,  0.146730f,  0.158858f,  0.170962f,  0.183040f,  0.195090f,  0.207111f,
    0.219101f,  0.231058f,  0.242980f,  0.254866f,  0.266713f,  0.278520f,  0.290285f,  0.302006f,  0.313682f,
    0.325310f,  0.336890f,  0.348419f,  0.359895f,  0.371317f,  0.382683f,  0.393992f,  0.405241f,  0.416430f,
    0.427555f,  0.438616f,  0.449611f,  0.460539f,  0.471397f,  0.482184f,  0.492898f,  0.503538f,  0.514103f,
    0.524590f,  0.534998f,  0.545325f,  0.555570f,  0.565732f,  0.575808f,  0.585798f,  0.595699f,  0.605511f,
    0.615232f,  0.624859f,  0.634393f,  0.643832f,  0.653173f,  0.662416f,  0.671559f,  0.680601f,  0.689541f,
    0.698376f,  0.707107f,  0.715731f,  0.724247f,  0.732654f,  0.740951f,  0.749136f,  0.757209f,  0.765167f,
    0.773010f,  0.780737f,  0.788346f,  0.795837f,  0.803208f,  0.810457f,  0.817585f,  0.824589f,  0.831470f,
    0.838225f,  0.844854f,  0.851355f,  0.857729f,  0.863973f,  0.870087f,  0.876070f,  0.881921f,  0.887640f,
    0.893224f,  0.898674f,  0.903989f,  0.909168f,  0.914210f,  0.919114f,  0.923880f,  0.928506f,  0.932993f,
    0.937339f,  0.941544f,  0.945607f,  0.949528f,  0.953306f,  0.956940f,  0.960431f,  0.963776f,  0.966976f,
    0.970031f,  0.972940f,  0.975702f,  0.978317f,  0.980785f,  0.983105f,  0.985278f,  0.987301f,  0.989177f,
    0.990903f,  0.992480f,  0.993907f,  0.995185f,  0.996313f,  0.997290f,  0.998118f,  0.998795f,  0.999322f,
    0.999699f,  0.999925f,  1.000000f,  0.999925f,  0.999699f,  0.999322f,  0.998795f,  0.998118f,  0.997290f,
    0.996313f,  0.995185f,  0.993907f,  0.992480f,  0.990903f,  0.989177f,  0.987301f,  0.985278f,  0.983105f,
    0.980785f,  0.978317f,  0.975702f,  0.972940f,  0.970031f,  0.966976f,  0.963776f,  0.960431f,  0.956940f,
    0.953306f,  0.949528f,  0.945607f,  0.941544f,  0.937339f,  0.932993f,  0.928506f,  0.923880f,  0.919114f,
    0.914210f,  0.909168f,  0.903989f,  0.898674f,  0.893224f,  0.887640f,  0.881921f,  0.876070f,  0.870087f,
    0.863973f,  0.857729f,  0.851355f,  0.844854f,  0.838225f,  0.831470f,  0.824589f,  0.817585f,  0.810457f,
    0.803208f,  0.795837f,  0.788346f,  0.780737f,  0.773010f,  0.765167f,  0.757209f,  0.749136f,  0.740951f,
    0.732654f,  0.724247f,  0.715731f,  0.707107f,  0.698376f,  0.689541f,  0.680601f,  0.671559f,  0.662416f,
    0.653173f,  0.643832f,  0.634393f,  0.624859f,  0.615232f,  0.605511f,  0.595699f,  0.585798f,  0.575808f,
    0.565732f,  0.555570f,  0.545325f,  0.534998f,  0.524590f,  0.514103f,  0.503538f,  0.492898f,  0.482184f,
    0.471397f,  0.460539f,  0.449611f,  0.438616f,  0.427555f,  0.416430f,  0.405241f,  0.393992f,  0.382683f,
    0.371317f,  0.359895f,  0.348419f,  0.336890f,  0.325310f,  0.313682f,  0.302006f,  0.290285f,  0.278520f,
    0.266713f,  0.254866f,  0.242980f,  0.231058f,  0.219101f,  0.207111f,  0.195090f,  0.183040f,  0.170962f,
    0.158858f,  0.146730f,  0.134581f,  0.122411f,  0.110222f,  0.098017f,  0.085797f,  0.073565f,  0.061321f,
    0.049068f,  0.036807f,  0.024541f,  0.012272f,  0.000000f,  -0.012272f, -0.024541f, -0.036807f, -0.049068f,
    -0.061321f, -0.073565f, -0.085797f, -0.098017f, -0.110222f, -0.122411f, -0.134581f, -0.146730f, -0.158858f,
    -0.170962f, -0.183040f, -0.195090f, -0.207111f, -0.219101f, -0.231058f, -0.242980f, -0.254866f, -0.266713f,
    -0.278520f, -0.290285f, -0.302006f, -0.313682f, -0.325310f, -0.336890f, -0.348419f, -0.359895f, -0.371317f,
    -0.382683f, -0.393992f, -0.405241f, -0.416430f, -0.427555f, -0.438616f, -0.449611f, -0.460539f, -0.471397f,
    -0.482184f, -0.492898f, -0.503538f, -0.514103f, -0.524590f, -0.534998f, -0.545325f, -0.555570f, -0.565732f,
    -0.575808f, -0.585798f, -0.595699f, -0.605511f, -0.615232f, -0.624859f, -0.634393f, -0.643832f, -0.653173f,
    -0.662416f, -0.671559f, -0.680601f, -0.689541f, -0.698376f, -0.707107f, -0.715731f, -0.724247f, -0.732654f,
    -0.740951f, -0.749136f, -0.757209f, -0.765167f, -0.773010f, -0.780737f, -0.788346f, -0.795837f, -0.803208f,
    -0.810457f, -0.817585f, -0.824589f, -0.831470f, -0.838225f, -0.844854f, -0.851355f, -0.857729f, -0.863973f,
    -0.870087f, -0.876070f, -0.881921f, -0.887640f, -0.893224f, -0.898674f, -0.903989f, -0.909168f, -0.914210f,
    -0.919114f, -0.923880f, -0.928506f, -0.932993f, -0.937339f, -0.941544f, -0.945607f, -0.949528f, -0.953306f,
    -0.956940f, -0.960431f, -0.963776f, -0.966976f, -0.970031f, -0.972940f, -0.975702f, -0.978317f, -0.980785f,
    -0.983105f, -0.985278f, -0.987301f, -0.989177f, -0.990903f, -0.992480f, -0.993907f, -0.995185f, -0.996313f,
    -0.997290f, -0.998118f, -0.998795f, -0.999322f, -0.999699f, -0.999925f, -1.000000f, -0.999925f, -0.999699f,
    -0.999322f, -0.998795f, -0.998118f, -0.997290f, -0.996313f, -0.995185f, -0.993907f, -0.992480f, -0.990903f,
    -0.989177f, -0.987301f, -0.985278f, -0.983105f, -0.980785f, -0.978317f, -0.975702f, -0.972940f, -0.970031f,
    -0.966976f, -0.963776f, -0.960431f, -0.956940f, -0.953306f, -0.949528f, -0.945607f, -0.941544f, -0.937339f,
    -0.932993f, -0.928506f, -0.923880f, -0.919114f, -0.914210f, -0.909168f, -0.903989f, -0.898674f, -0.893224f,
    -0.887640f, -0.881921f, -0.876070f, -0.870087f, -0.863973f, -0.857729f, -0.851355f, -0.844854f, -0.838225f,
    -0.831470f, -0.824589f, -0.817585f, -0.810457f, -0.803208f, -0.795837f, -0.788346f, -0.780737f, -0.773010f,
    -0.765167f, -0.757209f, -0.749136f, -0.740951f, -0.732654f, -0.724247f, -0.715731f, -0.707107f, -0.698376f,
    -0.689541f, -0.680601f, -0.671559f, -0.662416f, -0.653173f, -0.643832f, -0.634393f, -0.624859f, -0.615232f,
    -0.605511f, -0.595699f, -0.585798f, -0.575808f, -0.565732f, -0.555570f, -0.545325f, -0.534998f, -0.524590f,
    -0.514103f, -0.503538f, -0.492898f, -0.482184f, -0.471397f, -0.460539f, -0.449611f, -0.438616f, -0.427555f,
    -0.416430f, -0.405241f, -0.393992f, -0.382683f, -0.371317f, -0.359895f, -0.348419f, -0.336890f, -0.325310f,
    -0.313682f, -0.302006f, -0.290285f, -0.278520f, -0.266713f, -0.254866f, -0.242980f, -0.231058f, -0.219101f,
    -0.207111f, -0.195090f, -0.183040f, -0.170962f, -0.158858f, -0.146730f, -0.134581f, -0.122411f, -0.110222f,
    -0.098017f, -0.085797f, -0.073565f, -0.061321f, -0.049068f, -0.036807f, -0.024541f, -0.012272f,
};
#elif (SIN_TABLE_SIZE == 1024)
static const float sin_table[SIN_TABLE_SIZE] = {
    0.000000f,  0.006136f,  0.012272f,  0.018407f,  0.024541f,  0.030675f,  0.036807f,  0.042938f,  0.049068f,
    0.055195f,  0.061321f,  0.067444f,  0.073565f,  0.079682f,  0.085797f,  0.091909f,  0.098017f,  0.104122f,
    0.110222f,  0.116319f,  0.122411f,  0.128498f,  0.134581f,  0.140658f,  0.146730f,  0.152797f,  0.158858f,
    0.164913f,  0.170962f,  0.177004f,  0.183040f,  0.189069f,  0.195090f,  0.201105f,  0.207111f,  0.213110f,
    0.219101f,  0.225084f,  0.231058f,  0.237024f,  0.242980f,  0.248928f,  0.254866f,  0.260794f,  0.266713f,
    0.272621f,  0.278520f,  0.284408f,  0.290285f,  0.296151f,  0.302006f,  0.307850f,  0.313682f,  0.319502f,
    0.325310f,  0.331106f,  0.336890f,  0.342661f,  0.348419f,  0.354164f,  0.359895f,  0.365613f,  0.371317f,
    0.377007f,  0.382683f,  0.388345f,  0.393992f,  0.399624f,  0.405241f,  0.410843f,  0.416430f,  0.422000f,
    0.427555f,  0.433094f,  0.438616f,  0.444122f,  0.449611f,  0.455084f,  0.460539f,  0.465976f,  0.471397f,
    0.476799f,  0.482184f,  0.487550f,  0.492898f,  0.498228f,  0.503538f,  0.508830f,  0.514103f,  0.519356f,
    0.524590f,  0.529804f,  0.534998f,  0.540171f,  0.545325f,  0.550458f,  0.555570f,  0.560662f,  0.565732f,
    0.570781f,  0.575808f,  0.580814f,  0.585798f,  0.590760f,  0.595699f,  0.600616f,  0.605511f,  0.610383f,
    0.615232f,  0.620057f,  0.624859f,  0.629638f,  0.634393f,  0.639124f,  0.643832f,  0.648514f,  0.653173f,
    0.657807f,  0.662416f,  0.667000f,  0.671559f,  0.676093f,  0.680601f,  0.685084f,  0.689541f,  0.693971f,
    0.698376f,  0.702755f,  0.707107f,  0.711432f,  0.715731f,  0.720003f,  0.724247f,  0.728464f,  0.732654f,
    0.736817f,  0.740951f,  0.745058f,  0.749136f,  0.753187f,  0.757209f,  0.761202f,  0.765167f,  0.769103f,
    0.773010f,  0.776888f,  0.780737f,  0.784557f,  0.788346f,  0.792107f,  0.795837f,  0.799537f,  0.803208f,
    0.806848f,  0.810457f,  0.814036f,  0.817585f,  0.821103f,  0.824589f,  0.828045f,  0.831470f,  0.834863f,
    0.838225f,  0.841555f,  0.844854f,  0.848120f,  0.851355f,  0.854558f,  0.857729f,  0.860867f,  0.863973f,
    0.867046f,  0.870087f,  0.873095f,  0.876070f,  0.879012f,  0.881921f,  0.884797f,  0.887640f,  0.890449f,
    0.893224f,  0.895966f,  0.898674f,  0.901349f,  0.903989f,  0.906596f,  0.909168f,  0.911706f,  0.914210f,
    0.916679f,  0.919114f,  0.921514f,  0.923880f,  0.926210f,  0.928506f,  0.930767f,  0.932993f,  0.935184f,
    0.937339f,  0.939459f,  0.941544f,  0.943593f,  0.945607f,  0.947586f,  0.949528f,  0.951435f,  0.953306f,
    0.955141f,  0.956940f,  0.958703f,  0.960431f,  0.962121f,  0.963776f,  0.965394f,  0.966976f,  0.968522f,
    0.970031f,  0.971504f,  0.972940f,  0.974339f,  0.975702f,  0.977028f,  0.978317f,  0.979570f,  0.980785f,
    0.981964f,  0.983105f,  0.984210f,  0.985278f,  0.986308f,  0.987301f,  0.988258f,  0.989177f,  0.990058f,
    0.990903f,  0.991710f,  0.992480f,  0.993212f,  0.993907f,  0.994565f,  0.995185f,  0.995767f,  0.996313f,
    0.996820f,  0.997290f,  0.997723f,  0.998118f,  0.998476f,  0.998795f,  0.999078f,  0.999322f,  0.999529f,
    0.999699f,  0.999831f,  0.999925f,  0.999981f,  1.000000f,  0.999981f,  0.999925f,  0.999831f,  0.999699f,
    0.999529f,  0.999322f,  0.999078f,  0.998795f,  0.998476f,  0.998118f,  0.997723f,  0.997290f,  0.996820f,
    0.996313f,  0.995767f,  0.995185f,  0.994565f,  0.993907f,  0.993212f,  0.992480f,  0.991710f,  0.990903f,
    0.990058f,  0.989177f,  0.988258f,  0.987301f,  0.986308f,  0.985278f,  0.984210f,  0.983105f,  0.981964f,
    0.980785f,  0.979570f,  0.978317f,  0.977028f,  0.975702f,  0.974339f,  0.972940f,  0.971504f,  0.970031f,
    0.968522f,  0.966976f,  0.965394f,  0.963776f,  0.962121f,  0.960431f,  0.958703f,  0.956940f,  0.955141f,
    0.953306f,  0.951435f,  0.949528f,  0.947586f,  0.945607f,  0.943593f,  0.941544f,  0.939459f,  0.937339f,
    0.935184f,  0.932993f,  0.930767f,  0.928506f,  0.926210f,  0.923880f,  0.921514f,  0.919114f,  0.916679f,
    0.914210f,  0.911706f,  0.909168f,  0.906596f,  0.903989f,  0.901349f,  0.898674f,  0.895966f,  0.893224f,
    0.890449f,  0.887640f,  0.884797f,  0.881921f,  0.879012f,  0.876070f,  0.873095f,  0.870087f,  0.867046f,
    0.863973f,  0.860867f,  0.857729f,  0.854558f,  0.851355f,  0.848120f,  0.844854f,  0.841555f,  0.838225f,
    0.834863f,  0.831470f,  0.828045f,  0.824589f,  0.821103f,  0.817585f,  0.814036f,  0.810457f,  0.806848f,
    0.803208f,  0.799537f,  0.795837f,  0.792107f,  0.788346f,  0.784557f,  0.780737f,  0.776888f,  0.773010f,
    0.769103f,  0.765167f,  0.761202f,  0.757209f,  0.753187f,  0.749136f,  0.745058f,  0.740951f,  0.736817f,
    0.732654f,  0.728464f,  0.724247f,  0.720003f,  0.715731f,  0.711432f,  0.707107f,  0.702755f,  0.698376f,
    0.693971f,  0.689541f,  0.685084f,  0.680601f,  0.676093f,  0.671559f,  0.667000f,  0.662416f,  0.657807f,
    0.653173f,  0.648514f,  0.643832f,  0.639124f,  0.634393f,  0.629638f,  0.624859f,  0.620057f,  0.615232f,
    0.610383f,  0.605511f,  0.600616f,  0.595699f,  0.590760f,  0.585798f,  0.580814f,  0.575808f,  0.570781f,
    0.565732f,  0.560662f,  0.555570f,  0.550458f,  0.545325f,  0.540171f,  0.534998f,  0.529804f,  0.524590f,
    0.519356f,  0.514103f,  0.508830f,  0.503538f,  0.498228f,  0.492898f,  0.487550f,  0.482184f,  0.476799f,
    0.471397f,  0.465976f,  0.460539f,  0.455084f,  0.449611f,  0.444122f,  0.438616f,  0.433094f,  0.427555f,
    0.422000f,  0.416430f,  0.410843f,  0.405241f,  0.399624f,  0.393992f,  0.388345f,  0.382683f,  0.377007f,
    0.371317f,  0.365613f,  0.359895f,  0.354164f,  0.348419f,  0.342661f,  0.336890f,  0.331106f,  0.325310f,
    0.319502f,  0.313682f,  0.307850f,  0.302006f,  0.296151f,  0.290285f,  0.284408f,  0.278520f,  0.272621f,
    0.266713f,  0.260794f,  0.254866f,  0.248928f,  0.242980f,  0.237024f,  0.231058f,  0.225084f,  0.219101f,
    0.213110f,  0.207111f,  0.201105f,  0.195090f,  0.189069f,  0.183040f,  0.177004f,  0.170962f,  0.164913f,
    0.158858f,  0.152797f,  0.146730f,  0.140658f,  0.134581f,  0.128498f,  0.122411f,  0.116319f,  0.110222f,
    0.104122f,  0.098017f,  0.091909f,  0.085797f,  0.079682f,  0.073565f,  0.067444f,  0.061321f,  0.055195f,
    0.049068f,  0.042938f,  0.036807f,  0.030675f,  0.024541f,  0.018407f,  0.012272f,  0.006136f,  0.000000f,
    -0.006136f, -0.012272f, -0.018407f, -0.024541f, -0.030675f, -0.036807f, -0.042938f, -0.049068f, -0.055195f,
    -0.061321f, -0.067444f, -0.073565f, -0.079682f, -0.085797f, -0.091909f, -0.098017f, -0.104122f, -0.110222f,
    -0.116319f, -0.122411f, -0.128498f, -0.134581f, -0.140658f, -0.146730f, -0.152797f, -0.158858f, -0.164913f,
    -0.170962f, -0.177004f, -0.183040f, -0.189069f, -0.195090f, -0.201105f, -0.207111f, -0.213110f, -0.219101f,
    -0.225084f, -0.231058f, -0.237024f, -0.242980f, -0.248928f, -0.254866f, -0.260794f, -0.266713f, -0.272621f,
    -0.278520f, -0.284408f, -0.290285f, -0.296151f, -0.302006f, -0.307850f, -0.313682f, -0.319502f, -0.325310f,
    -0.331106f, -0.336890f, -0.342661f, -0.348419f, -0.354164f, -0.359895f, -0.365613f, -0.371317f, -0.377007f,
    -0.382683f, -0.388345f, -0.393992f, -0.399624f, -0.405241f, -0.410843f, -0.416430f, -0.422000f, -0.427555f,
    -0.433094f, -0.438616f, -0.444122f, -0.449611f, -0.455084f, -0.460539f, -0.465976f, -0.471397f, -0.476799f,
    -0.482184f, -0.487550f, -0.492898f, -0.498228f, -0.503538f, -0.508830f, -0.514103f, -0.519356f, -0.524590f,
    -0.529804f, -0.534998f, -0.540171f, -0.545325f, -0.550458f, -0.555570f, -0.560662f, -0.565732f, -0.570781f,
    -0.575808f, -0.580814f, -0.585798f, -0.590760f, -0.595699f, -0.600616f, -0.605511f, -0.610383f, -0.615232f,
    -0.620057f, -0.624859f, -0.629638f, -0.634393f, -0.639124f, -0.643832f, -0.648514f, -0.653173f, -0.657807f,
    -0.662416f, -0.667000f, -0.671559f, -0.676093f, -0.680601f, -0.685084f, -0.689541f, -0.693971f, -0.698376f,
    -0.702755f, -0.707107f, -0.711432f, -0.715731f, -0.720003f, -0.724247f, -0.728464f, -0.732654f, -0.736817f,
    -0.740951f, -0.745058f, -0.749136f, -0.753187f, -0.757209f, -0.761202f, -0.765167f, -0.769103f, -0.773010f,
    -0.776888f, -0.780737f, -0.784557f, -0.788346f, -0.792107f, -0.795837f, -0.799537f, -0.803208f, -0.806848f,
    -0.810457f, -0.814036f, -0.817585f, -0.821103f, -0.824589f, -0.828045f, -0.831470f, -0.834863f, -0.838225f,
    -0.841555f, -0.844854f, -0.848120f, -0.851355f, -0.854558f, -0.857729f, -0.860867f, -0.863973f, -0.867046f,
    -0.870087f, -0.873095f, -0.876070f, -0.879012f, -0.881921f, -0.884797f, -0.887640f, -0.890449f, -0.893224f,
    -0.895966f, -0.898674f, -0.901349f, -0.903989f, -0.906596f, -0.909168f, -0.911706f, -0.914210f, -0.916679f,
    -0.919114f, -0.921514f, -0.923880f, -0.926210f, -0.928506f, -0.930767f, -0.932993f, -0.935184f, -0.937339f,
    -0.939459f, -0.941544f, -0.943593f, -0.945607f, -0.947586f, -0.949528f, -0.951435f, -0.953306f, -0.955141f,
    -0.956940f, -0.958703f, -0.960431f, -0.962121f, -0.963776f, -0.965394f, -0.966976f, -0.968522f, -0.970031f,
    -0.971504f, -0.972940f, -0.974339f, -0.975702f, -0.977028f, -0.978317f, -0.979570f, -0.980785f, -0.981964f,
    -0.983105f, -0.984210f, -0.985278f, -0.986308f, -0.987301f, -0.988258f, -0.989177f, -0.990058f, -0.990903f,
    -0.991710f, -0.992480f, -0.993212f, -0.993907f, -0.994565f, -0.995185f, -0.995767f, -0.996313f, -0.996820f,
    -0.997290f, -0.997723f, -0.998118f, -0.998476f, -0.998795f, -0.999078f, -0.999322f, -0.999529f, -0.999699f,
    -0.999831f, -0.999925f, -0.999981f, -1.000000f, -0.999981f, -0.999925f, -0.999831f, -0.999699f, -0.999529f,
    -0.999322f, -0.999078f, -0.998795f, -0.998476f, -0.998118f, -0.997723f, -0.997290f, -0.996820f, -0.996313f,
    -0.995767f, -0.995185f, -0.994565f, -0.993907f, -0.993212f, -0.992480f, -0.991710f, -0.990903f, -0.990058f,
    -0.989177f, -0.988258f, -0.987301f, -0.986308f, -0.985278f, -0.984210f, -0.983105f, -0.981964f, -0.980785f,
    -0.979570f, -0.978317f, -0.977028f, -0.975702f, -0.974339f, -0.972940f, -0.971504f, -0.970031f, -0.968522f,
    -0.966976f, -0.965394f, -0.963776f, -0.962121f, -0.960431f, -0.958703f, -0.956940f, -0.955141f, -0.953306f,
    -0.951435f, -0.949528f, -0.947586f, -0.945607f, -0.943593f, -0.941544f, -0.939459f, -0.937339f, -0.935184f,
    -0.932993f, -0.930767f, -0.928506f, -0.926210f, -0.923880f, -0.921514f, -0.919114f, -0.916679f, -0.914210f,
    -0.911706f, -0.909168f, -0.906596f, -0.903989f, -0.901349f, -0.898674f, -0.895966f, -0.893224f, -0.890449f,
    -0.887640f, -0.884797f, -0.881921f, -0.879012f, -0.876070f, -0.873095f, -0.870087f, -0.867046f, -0.863973f,
    -0.860867f, -0.857729f, -0.854558f, -0.851355f, -0.848120f, -0.844854f, -0.841555f, -0.838225f, -0.834863f,
    -0.831470f, -0.828045f, -0.824589f, -0.821103f, -0.817585f, -0.814036f, -0.810457f, -0.806848f, -0.803208f,
    -0.799537f, -0.795837f, -0.792107f, -0.788346f, -0.784557f, -0.780737f, -0.776888f, -0.773010f, -0.769103f,
    -0.765167f, -0.761202f, -0.757209f, -0.753187f, -0.749136f, -0.745058f, -0.740951f, -0.736817f, -0.732654f,
    -0.728464f, -0.724247f, -0.720003f, -0.715731f, -0.711432f, -0.707107f, -0.702755f, -0.698376f, -0.693971f,
    -0.689541f, -0.685084f, -0.680601f, -0.676093f, -0.671559f, -0.667000f, -0.662416f, -0.657807f, -0.653173f,
    -0.648514f, -0.643832f, -0.639124f, -0.634393f, -0.629638f, -0.624859f, -0.620057f, -0.615232f, -0.610383f,
    -0.605511f, -0.600616f, -0.595699f, -0.590760f, -0.585798f, -0.580814f, -0.575808f, -0.570781f, -0.565732f,
    -0.560662f, -0.555570f, -0.550458f, -0.545325f, -0.540171f, -0.534998f, -0.529804f, -0.524590f, -0.519356f,
    -0.514103f, -0.508830f, -0.503538f, -0.498228f, -0.492898f, -0.487550f, -0.482184f, -0.476799f, -0.471397f,
    -0.465976f, -0.460539f, -0.455084f, -0.449611f, -0.444122f, -0.438616f, -0.433094f, -0.427555f, -0.422000f,
    -0.416430f, -0.410843f, -0.405241f, -0.399624f, -0.393992f, -0.388345f, -0.382683f, -0.377007f, -0.371317f,
    -0.365613f, -0.359895f, -0.354164f, -0.348419f, -0.342661f, -0.336890f, -0.331106f, -0.325310f, -0.319502f,
    -0.313682f, -0.307850f, -0.302006f, -0.296151f, -0.290285f, -0.284408f, -0.278520f, -0.272621f, -0.266713f,
    -0.260794f, -0.254866f, -0.248928f, -0.242980f, -0.237024f, -0.231058f, -0.225084f, -0.219101f, -0.213110f,
    -0.207111f, -0.201105f, -0.195090f, -0.189069f, -0.183040f, -0.177004f, -0.170962f, -0.164913f, -0.158858f,
    -0.152797f, -0.146730f, -0.140658f, -0.134581f, -0.128498f, -0.122411f, -0.116319f, -0.110222f, -0.104122f,
    -0.098017f, -0.091909f, -0.085797f, -0.079682f, -0.073565f, -0.067444f, -0.061321f, -0.055195f, -0.049068f,
    -0.042938f, -0.036807f, -0.030675f, -0.024541f, -0.018407f, -0.012272f, -0.006136f,
};
#endif

/**************ʵ�ֺ���****************/
#if (SIN_LINER_INTER_EN == 1) /**************ʹ�ò������������ֵ���������Բ�ֵ****************/
float fast_sin(float x)
{
    int16_t index, next_index;
    float   index_f, frac;

    // ����һ����ĽǶ�ת��Ϊ���������
    index_f = (_norm_angle(x) * _1_D2PI) * SIN_TABLE_SIZE;

    // ��ȡ����������С������
    index = (int16_t)index_f;
    frac = index_f - index;

    // �����߽�������������Բ�ֵ
    next_index = (index + 1) % SIN_TABLE_SIZE;
    return sin_table[index] + frac * (sin_table[next_index] - sin_table[index]);
}
#else /**************ʹ�ò������������ֵ****************/
float fast_sin(float x)
{
    // ���Ƕ�ת��Ϊ���������
    int16_t index;
    index = (int16_t)((_norm_angle(x) * _1_D2PI) * SIN_TABLE_SIZE) % SIN_TABLE_SIZE;
    if (index < 0)
    {
        index += SIN_TABLE_SIZE;
    } // �������Ƕ�
    return sin_table[index];
}
#endif
/*================================================================�ֽ���======================================================================*/
/**************�������cos****************/
// ʹ�ÿ��ٷ�����������ֵ
float fast_cos(float x) { return fast_sin(x + _PI_D2); }

void fast_sin_cos(float x, float *sin_x, float *cos_x)
{
    *sin_x = fast_sin(x);
    *cos_x = fast_cos(x);
}
#endif

#if (ATAN_MODE == 1)
// �����к������ұ�
#if (ATAN_TABLE_SIZE == 512)
static const float atan_table[ATAN_TABLE_SIZE] = {
    0.00000000f, 0.00195694f, 0.00391387f, 0.00587077f, 0.00782763f, 0.00978442f, 0.01174114f, 0.01369777f, 0.01565430f,
    0.01761070f, 0.01956697f, 0.02152309f, 0.02347905f, 0.02543483f, 0.02739041f, 0.02934578f, 0.03130093f, 0.03325584f,
    0.03521049f, 0.03716488f, 0.03911898f, 0.04107278f, 0.04302627f, 0.04497943f, 0.04693224f, 0.04888470f, 0.05083679f,
    0.05278848f, 0.05473978f, 0.05669066f, 0.05864110f, 0.06059110f, 0.06254064f, 0.06448970f, 0.06643828f, 0.06838634f,
    0.07033389f, 0.07228090f, 0.07422737f, 0.07617327f, 0.07811859f, 0.08006332f, 0.08200745f, 0.08395095f, 0.08589382f,
    0.08783603f, 0.08977759f, 0.09171846f, 0.09365864f, 0.09559812f, 0.09753687f, 0.09947489f, 0.10141216f, 0.10334867f,
    0.10528440f, 0.10721933f, 0.10915346f, 0.11108677f, 0.11301925f, 0.11495088f, 0.11688165f, 0.11881155f, 0.12074055f,
    0.12266866f, 0.12459584f, 0.12652210f, 0.12844741f, 0.13037177f, 0.13229515f, 0.13421756f, 0.13613896f, 0.13805935f,
    0.13997872f, 0.14189705f, 0.14381434f, 0.14573055f, 0.14764569f, 0.14955974f, 0.15147269f, 0.15338451f, 0.15529521f,
    0.15720477f, 0.15911317f, 0.16102041f, 0.16292646f, 0.16483132f, 0.16673497f, 0.16863740f, 0.17053861f, 0.17243856f,
    0.17433727f, 0.17623470f, 0.17813085f, 0.18002571f, 0.18191926f, 0.18381149f, 0.18570239f, 0.18759195f, 0.18948016f,
    0.19136700f, 0.19325246f, 0.19513653f, 0.19701920f, 0.19890045f, 0.20078028f, 0.20265867f, 0.20453561f, 0.20641109f,
    0.20828510f, 0.21015763f, 0.21202866f, 0.21389819f, 0.21576620f, 0.21763268f, 0.21949762f, 0.22136101f, 0.22322284f,
    0.22508309f, 0.22694177f, 0.22879885f, 0.23065432f, 0.23250818f, 0.23436041f, 0.23621101f, 0.23805995f, 0.23990725f,
    0.24175287f, 0.24359681f, 0.24543907f, 0.24727963f, 0.24911847f, 0.25095560f, 0.25279100f, 0.25462467f, 0.25645658f,
    0.25828673f, 0.26011512f, 0.26194173f, 0.26376655f, 0.26558958f, 0.26741080f, 0.26923020f, 0.27104778f, 0.27286352f,
    0.27467742f, 0.27648947f, 0.27829966f, 0.28010798f, 0.28191441f, 0.28371896f, 0.28552161f, 0.28732236f, 0.28912119f,
    0.29091810f, 0.29271308f, 0.29450611f, 0.29629720f, 0.29808633f, 0.29987350f, 0.30165870f, 0.30344191f, 0.30522313f,
    0.30700236f, 0.30877958f, 0.31055479f, 0.31232798f, 0.31409914f, 0.31586827f, 0.31763535f, 0.31940038f, 0.32116336f,
    0.32292426f, 0.32468310f, 0.32643985f, 0.32819452f, 0.32994709f, 0.33169756f, 0.33344593f, 0.33519218f, 0.33693630f,
    0.33867830f, 0.34041816f, 0.34215589f, 0.34389146f, 0.34562488f, 0.34735613f, 0.34908522f, 0.35081214f, 0.35253687f,
    0.35425942f, 0.35597978f, 0.35769794f, 0.35941389f, 0.36112764f, 0.36283917f, 0.36454848f, 0.36625556f, 0.36796041f,
    0.36966302f, 0.37136339f, 0.37306151f, 0.37475737f, 0.37645098f, 0.37814232f, 0.37983139f, 0.38151818f, 0.38320270f,
    0.38488493f, 0.38656487f, 0.38824252f, 0.38991786f, 0.39159091f, 0.39326164f, 0.39493007f, 0.39659617f, 0.39825996f,
    0.39992141f, 0.40158054f, 0.40323733f, 0.40489179f, 0.40654390f, 0.40819366f, 0.40984107f, 0.41148613f, 0.41312883f,
    0.41476917f, 0.41640714f, 0.41804274f, 0.41967597f, 0.42130682f, 0.42293529f, 0.42456138f, 0.42618508f, 0.42780639f,
    0.42942531f, 0.43104183f, 0.43265595f, 0.43426766f, 0.43587697f, 0.43748387f, 0.43908836f, 0.44069044f, 0.44229010f,
    0.44388734f, 0.44548215f, 0.44707454f, 0.44866450f, 0.45025204f, 0.45183713f, 0.45341980f, 0.45500003f, 0.45657781f,
    0.45815316f, 0.45972606f, 0.46129652f, 0.46286452f, 0.46443008f, 0.46599319f, 0.46755384f, 0.46911204f, 0.47066777f,
    0.47222105f, 0.47377187f, 0.47532023f, 0.47686612f, 0.47840955f, 0.47995051f, 0.48148901f, 0.48302503f, 0.48455858f,
    0.48608966f, 0.48761827f, 0.48914440f, 0.49066806f, 0.49218924f, 0.49370794f, 0.49522417f, 0.49673791f, 0.49824918f,
    0.49975796f, 0.50126426f, 0.50276809f, 0.50426942f, 0.50576828f, 0.50726464f, 0.50875853f, 0.51024993f, 0.51173884f,
    0.51322527f, 0.51470921f, 0.51619067f, 0.51766963f, 0.51914611f, 0.52062011f, 0.52209161f, 0.52356063f, 0.52502716f,
    0.52649121f, 0.52795276f, 0.52941183f, 0.53086842f, 0.53232251f, 0.53377412f, 0.53522324f, 0.53666988f, 0.53811403f,
    0.53955569f, 0.54099487f, 0.54243157f, 0.54386578f, 0.54529751f, 0.54672676f, 0.54815352f, 0.54957780f, 0.55099960f,
    0.55241892f, 0.55383577f, 0.55525013f, 0.55666202f, 0.55807143f, 0.55947836f, 0.56088282f, 0.56228480f, 0.56368431f,
    0.56508135f, 0.56647592f, 0.56786803f, 0.56925766f, 0.57064482f, 0.57202952f, 0.57341176f, 0.57479153f, 0.57616884f,
    0.57754369f, 0.57891607f, 0.58028601f, 0.58165348f, 0.58301850f, 0.58438107f, 0.58574119f, 0.58709885f, 0.58845407f,
    0.58980684f, 0.59115717f, 0.59250505f, 0.59385049f, 0.59519349f, 0.59653405f, 0.59787218f, 0.59920787f, 0.60054113f,
    0.60187196f, 0.60320036f, 0.60452633f, 0.60584988f, 0.60717101f, 0.60848971f, 0.60980600f, 0.61111987f, 0.61243133f,
    0.61374037f, 0.61504701f, 0.61635123f, 0.61765305f, 0.61895247f, 0.62024949f, 0.62154410f, 0.62283632f, 0.62412615f,
    0.62541359f, 0.62669863f, 0.62798129f, 0.62926156f, 0.63053946f, 0.63181497f, 0.63308811f, 0.63435887f, 0.63562726f,
    0.63689328f, 0.63815693f, 0.63941822f, 0.64067714f, 0.64193371f, 0.64318792f, 0.64443978f, 0.64568929f, 0.64693645f,
    0.64818126f, 0.64942373f, 0.65066386f, 0.65190166f, 0.65313712f, 0.65437025f, 0.65560105f, 0.65682952f, 0.65805567f,
    0.65927950f, 0.66050102f, 0.66172022f, 0.66293711f, 0.66415169f, 0.66536396f, 0.66657394f, 0.66778161f, 0.66898699f,
    0.67019008f, 0.67139087f, 0.67258938f, 0.67378560f, 0.67497955f, 0.67617122f, 0.67736061f, 0.67854773f, 0.67973258f,
    0.68091517f, 0.68209550f, 0.68327356f, 0.68444938f, 0.68562294f, 0.68679425f, 0.68796331f, 0.68913014f, 0.69029472f,
    0.69145707f, 0.69261718f, 0.69377507f, 0.69493073f, 0.69608417f, 0.69723539f, 0.69838439f, 0.69953118f, 0.70067577f,
    0.70181814f, 0.70295832f, 0.70409629f, 0.70523207f, 0.70636566f, 0.70749706f, 0.70862627f, 0.70975330f, 0.71087816f,
    0.71200084f, 0.71312135f, 0.71423969f, 0.71535586f, 0.71646988f, 0.71758173f, 0.71869144f, 0.71979899f, 0.72090440f,
    0.72200766f, 0.72310879f, 0.72420777f, 0.72530463f, 0.72639936f, 0.72749196f, 0.72858244f, 0.72967080f, 0.73075705f,
    0.73184118f, 0.73292321f, 0.73400313f, 0.73508096f, 0.73615669f, 0.73723032f, 0.73830187f, 0.73937133f, 0.74043871f,
    0.74150401f, 0.74256723f, 0.74362838f, 0.74468747f, 0.74574449f, 0.74679946f, 0.74785236f, 0.74890322f, 0.74995202f,
    0.75099878f, 0.75204350f, 0.75308618f, 0.75412683f, 0.75516544f, 0.75620203f, 0.75723660f, 0.75826914f, 0.75929967f,
    0.76032819f, 0.76135470f, 0.76237921f, 0.76340171f, 0.76442222f, 0.76544073f, 0.76645726f, 0.76747180f, 0.76848435f,
    0.76949493f, 0.77050354f, 0.77151017f, 0.77251483f, 0.77351753f, 0.77451828f, 0.77551706f, 0.77651390f, 0.77750878f,
    0.77850172f, 0.77949272f, 0.78048178f, 0.78146891f, 0.78245411f, 0.78343738f, 0.78441873f, 0.78539816f};
#elif (ATAN_TABLE_SIZE == 1024)
static const float atan_table[ATAN_TABLE_SIZE] = {
    0.00000000f, 0.00097752f, 0.00195503f, 0.00293254f, 0.00391005f, 0.00488755f, 0.00586504f, 0.00684251f, 0.00781998f,
    0.00879743f, 0.00977486f, 0.01075227f, 0.01172967f, 0.01270704f, 0.01368439f, 0.01466171f, 0.01563900f, 0.01661626f,
    0.01759349f, 0.01857069f, 0.01954785f, 0.02052498f, 0.02150206f, 0.02247911f, 0.02345611f, 0.02443306f, 0.02540997f,
    0.02638684f, 0.02736365f, 0.02834041f, 0.02931711f, 0.03029376f, 0.03127035f, 0.03224688f, 0.03322335f, 0.03419976f,
    0.03517610f, 0.03615237f, 0.03712858f, 0.03810471f, 0.03908078f, 0.04005676f, 0.04103267f, 0.04200851f, 0.04298426f,
    0.04395993f, 0.04493552f, 0.04591102f, 0.04688643f, 0.04786176f, 0.04883699f, 0.04981213f, 0.05078718f, 0.05176213f,
    0.05273698f, 0.05371173f, 0.05468638f, 0.05566092f, 0.05663536f, 0.05760969f, 0.05858391f, 0.05955802f, 0.06053202f,
    0.06150590f, 0.06247967f, 0.06345331f, 0.06442684f, 0.06540024f, 0.06637352f, 0.06734668f, 0.06831970f, 0.06929260f,
    0.07026536f, 0.07123800f, 0.07221049f, 0.07318285f, 0.07415507f, 0.07512716f, 0.07609909f, 0.07707089f, 0.07804254f,
    0.07901404f, 0.07998539f, 0.08095659f, 0.08192764f, 0.08289853f, 0.08386927f, 0.08483985f, 0.08581026f, 0.08678052f,
    0.08775061f, 0.08872054f, 0.08969030f, 0.09065989f, 0.09162931f, 0.09259855f, 0.09356762f, 0.09453652f, 0.09550524f,
    0.09647378f, 0.09744213f, 0.09841030f, 0.09937829f, 0.10034609f, 0.10131371f, 0.10228113f, 0.10324836f, 0.10421539f,
    0.10518224f, 0.10614888f, 0.10711532f, 0.10808157f, 0.10904761f, 0.11001344f, 0.11097908f, 0.11194450f, 0.11290971f,
    0.11387471f, 0.11483950f, 0.11580408f, 0.11676843f, 0.11773257f, 0.11869649f, 0.11966019f, 0.12062367f, 0.12158692f,
    0.12254994f, 0.12351274f, 0.12447530f, 0.12543764f, 0.12639974f, 0.12736160f, 0.12832323f, 0.12928462f, 0.13024577f,
    0.13120667f, 0.13216734f, 0.13312775f, 0.13408792f, 0.13504785f, 0.13600752f, 0.13696694f, 0.13792610f, 0.13888502f,
    0.13984367f, 0.14080206f, 0.14176020f, 0.14271807f, 0.14367568f, 0.14463303f, 0.14559010f, 0.14654691f, 0.14750345f,
    0.14845972f, 0.14941571f, 0.15037143f, 0.15132687f, 0.15228203f, 0.15323692f, 0.15419152f, 0.15514584f, 0.15609987f,
    0.15705362f, 0.15800708f, 0.15896024f, 0.15991312f, 0.16086571f, 0.16181800f, 0.16276999f, 0.16372169f, 0.16467309f,
    0.16562419f, 0.16657498f, 0.16752548f, 0.16847566f, 0.16942554f, 0.17037511f, 0.17132437f, 0.17227332f, 0.17322195f,
    0.17417028f, 0.17511828f, 0.17606597f, 0.17701333f, 0.17796038f, 0.17890710f, 0.17985350f, 0.18079957f, 0.18174532f,
    0.18269074f, 0.18363583f, 0.18458058f, 0.18552500f, 0.18646909f, 0.18741284f, 0.18835626f, 0.18929933f, 0.19024207f,
    0.19118446f, 0.19212651f, 0.19306821f, 0.19400957f, 0.19495058f, 0.19589124f, 0.19683154f, 0.19777150f, 0.19871110f,
    0.19965035f, 0.20058924f, 0.20152777f, 0.20246594f, 0.20340375f, 0.20434120f, 0.20527828f, 0.20621500f, 0.20715135f,
    0.20808733f, 0.20902294f, 0.20995818f, 0.21089305f, 0.21182755f, 0.21276167f, 0.21369541f, 0.21462877f, 0.21556176f,
    0.21649436f, 0.21742658f, 0.21835842f, 0.21928987f, 0.22022094f, 0.22115161f, 0.22208190f, 0.22301180f, 0.22394130f,
    0.22487042f, 0.22579913f, 0.22672746f, 0.22765538f, 0.22858290f, 0.22951003f, 0.23043675f, 0.23136307f, 0.23228899f,
    0.23321450f, 0.23413960f, 0.23506430f, 0.23598859f, 0.23691246f, 0.23783593f, 0.23875898f, 0.23968161f, 0.24060383f,
    0.24152564f, 0.24244702f, 0.24336799f, 0.24428853f, 0.24520865f, 0.24612835f, 0.24704763f, 0.24796647f, 0.24888489f,
    0.24980288f, 0.25072045f, 0.25163758f, 0.25255428f, 0.25347054f, 0.25438637f, 0.25530177f, 0.25621672f, 0.25713124f,
    0.25804532f, 0.25895896f, 0.25987215f, 0.26078491f, 0.26169722f, 0.26260908f, 0.26352049f, 0.26443146f, 0.26534198f,
    0.26625205f, 0.26716167f, 0.26807083f, 0.26897954f, 0.26988780f, 0.27079559f, 0.27170294f, 0.27260982f, 0.27351624f,
    0.27442221f, 0.27532771f, 0.27623275f, 0.27713732f, 0.27804143f, 0.27894507f, 0.27984824f, 0.28075095f, 0.28165319f,
    0.28255495f, 0.28345625f, 0.28435707f, 0.28525741f, 0.28615728f, 0.28705668f, 0.28795560f, 0.28885404f, 0.28975200f,
    0.29064947f, 0.29154647f, 0.29244299f, 0.29333902f, 0.29423457f, 0.29512963f, 0.29602420f, 0.29691829f, 0.29781188f,
    0.29870499f, 0.29959761f, 0.30048973f, 0.30138136f, 0.30227250f, 0.30316314f, 0.30405329f, 0.30494294f, 0.30583209f,
    0.30672074f, 0.30760889f, 0.30849654f, 0.30938369f, 0.31027034f, 0.31115648f, 0.31204212f, 0.31292725f, 0.31381188f,
    0.31469600f, 0.31557961f, 0.31646271f, 0.31734530f, 0.31822737f, 0.31910894f, 0.31998999f, 0.32087053f, 0.32175055f,
    0.32263006f, 0.32350905f, 0.32438752f, 0.32526548f, 0.32614291f, 0.32701983f, 0.32789622f, 0.32877209f, 0.32964744f,
    0.33052226f, 0.33139656f, 0.33227033f, 0.33314358f, 0.33401629f, 0.33488848f, 0.33576015f, 0.33663128f, 0.33750188f,
    0.33837195f, 0.33924148f, 0.34011048f, 0.34097895f, 0.34184689f, 0.34271429f, 0.34358115f, 0.34444747f, 0.34531326f,
    0.34617850f, 0.34704321f, 0.34790738f, 0.34877100f, 0.34963409f, 0.35049663f, 0.35135862f, 0.35222008f, 0.35308098f,
    0.35394135f, 0.35480116f, 0.35566043f, 0.35651915f, 0.35737732f, 0.35823494f, 0.35909201f, 0.35994853f, 0.36080449f,
    0.36165991f, 0.36251477f, 0.36336908f, 0.36422283f, 0.36507603f, 0.36592867f, 0.36678075f, 0.36763228f, 0.36848325f,
    0.36933366f, 0.37018351f, 0.37103280f, 0.37188153f, 0.37272970f, 0.37357730f, 0.37442434f, 0.37527082f, 0.37611674f,
    0.37696209f, 0.37780687f, 0.37865109f, 0.37949474f, 0.38033783f, 0.38118034f, 0.38202229f, 0.38286367f, 0.38370448f,
    0.38454472f, 0.38538439f, 0.38622348f, 0.38706201f, 0.38789996f, 0.38873734f, 0.38957414f, 0.39041037f, 0.39124602f,
    0.39208110f, 0.39291560f, 0.39374953f, 0.39458287f, 0.39541564f, 0.39624783f, 0.39707945f, 0.39791048f, 0.39874093f,
    0.39957080f, 0.40040009f, 0.40122880f, 0.40205692f, 0.40288447f, 0.40371143f, 0.40453780f, 0.40536360f, 0.40618880f,
    0.40701342f, 0.40783746f, 0.40866091f, 0.40948377f, 0.41030605f, 0.41112773f, 0.41194883f, 0.41276934f, 0.41358926f,
    0.41440859f, 0.41522734f, 0.41604549f, 0.41686305f, 0.41768001f, 0.41849639f, 0.41931217f, 0.42012736f, 0.42094196f,
    0.42175596f, 0.42256937f, 0.42338219f, 0.42419441f, 0.42500603f, 0.42581706f, 0.42662749f, 0.42743733f, 0.42824657f,
    0.42905521f, 0.42986325f, 0.43067070f, 0.43147754f, 0.43228379f, 0.43308944f, 0.43389449f, 0.43469894f, 0.43550278f,
    0.43630603f, 0.43710867f, 0.43791072f, 0.43871216f, 0.43951300f, 0.44031324f, 0.44111287f, 0.44191190f, 0.44271033f,
    0.44350815f, 0.44430537f, 0.44510198f, 0.44589799f, 0.44669340f, 0.44748820f, 0.44828239f, 0.44907597f, 0.44986895f,
    0.45066133f, 0.45145309f, 0.45224425f, 0.45303480f, 0.45382474f, 0.45461408f, 0.45540280f, 0.45619092f, 0.45697843f,
    0.45776532f, 0.45855161f, 0.45933729f, 0.46012236f, 0.46090682f, 0.46169066f, 0.46247390f, 0.46325653f, 0.46403854f,
    0.46481994f, 0.46560073f, 0.46638091f, 0.46716048f, 0.46793943f, 0.46871777f, 0.46949550f, 0.47027261f, 0.47104911f,
    0.47182500f, 0.47260027f, 0.47337493f, 0.47414897f, 0.47492240f, 0.47569522f, 0.47646742f, 0.47723901f, 0.47800998f,
    0.47878033f, 0.47955007f, 0.48031919f, 0.48108770f, 0.48185559f, 0.48262287f, 0.48338953f, 0.48415557f, 0.48492100f,
    0.48568580f, 0.48645000f, 0.48721357f, 0.48797653f, 0.48873887f, 0.48950059f, 0.49026169f, 0.49102218f, 0.49178205f,
    0.49254130f, 0.49329993f, 0.49405795f, 0.49481534f, 0.49557212f, 0.49632828f, 0.49708382f, 0.49783874f, 0.49859304f,
    0.49934672f, 0.50009979f, 0.50085223f, 0.50160405f, 0.50235526f, 0.50310585f, 0.50385581f, 0.50460516f, 0.50535389f,
    0.50610199f, 0.50684948f, 0.50759635f, 0.50834259f, 0.50908822f, 0.50983323f, 0.51057762f, 0.51132138f, 0.51206453f,
    0.51280705f, 0.51354896f, 0.51429025f, 0.51503091f, 0.51577096f, 0.51651038f, 0.51724918f, 0.51798737f, 0.51872493f,
    0.51946187f, 0.52019819f, 0.52093389f, 0.52166897f, 0.52240343f, 0.52313727f, 0.52387049f, 0.52460309f, 0.52533506f,
    0.52606642f, 0.52679715f, 0.52752727f, 0.52825676f, 0.52898564f, 0.52971389f, 0.53044152f, 0.53116854f, 0.53189493f,
    0.53262070f, 0.53334585f, 0.53407038f, 0.53479429f, 0.53551758f, 0.53624025f, 0.53696230f, 0.53768373f, 0.53840453f,
    0.53912472f, 0.53984429f, 0.54056324f, 0.54128157f, 0.54199928f, 0.54271637f, 0.54343284f, 0.54414869f, 0.54486392f,
    0.54557853f, 0.54629252f, 0.54700589f, 0.54771864f, 0.54843077f, 0.54914229f, 0.54985318f, 0.55056346f, 0.55127312f,
    0.55198215f, 0.55269057f, 0.55339837f, 0.55410556f, 0.55481212f, 0.55551807f, 0.55622340f, 0.55692811f, 0.55763220f,
    0.55833567f, 0.55903853f, 0.55974077f, 0.56044239f, 0.56114339f, 0.56184378f, 0.56254355f, 0.56324270f, 0.56394124f,
    0.56463916f, 0.56533646f, 0.56603315f, 0.56672922f, 0.56742467f, 0.56811951f, 0.56881373f, 0.56950734f, 0.57020033f,
    0.57089271f, 0.57158447f, 0.57227561f, 0.57296614f, 0.57365606f, 0.57434536f, 0.57503405f, 0.57572212f, 0.57640958f,
    0.57709642f, 0.57778266f, 0.57846827f, 0.57915328f, 0.57983767f, 0.58052145f, 0.58120461f, 0.58188716f, 0.58256910f,
    0.58325043f, 0.58393115f, 0.58461125f, 0.58529075f, 0.58596963f, 0.58664790f, 0.58732556f, 0.58800260f, 0.58867904f,
    0.58935487f, 0.59003008f, 0.59070469f, 0.59137869f, 0.59205208f, 0.59272485f, 0.59339702f, 0.59406858f, 0.59473953f,
    0.59540988f, 0.59607961f, 0.59674874f, 0.59741725f, 0.59808517f, 0.59875247f, 0.59941917f, 0.60008526f, 0.60075074f,
    0.60141562f, 0.60207989f, 0.60274355f, 0.60340661f, 0.60406906f, 0.60473091f, 0.60539215f, 0.60605279f, 0.60671283f,
    0.60737226f, 0.60803108f, 0.60868931f, 0.60934693f, 0.61000394f, 0.61066036f, 0.61131617f, 0.61197138f, 0.61262598f,
    0.61327999f, 0.61393339f, 0.61458619f, 0.61523839f, 0.61588999f, 0.61654099f, 0.61719139f, 0.61784119f, 0.61849039f,
    0.61913899f, 0.61978699f, 0.62043440f, 0.62108120f, 0.62172741f, 0.62237302f, 0.62301803f, 0.62366244f, 0.62430626f,
    0.62494948f, 0.62559210f, 0.62623413f, 0.62687556f, 0.62751640f, 0.62815664f, 0.62879629f, 0.62943534f, 0.63007380f,
    0.63071166f, 0.63134893f, 0.63198561f, 0.63262169f, 0.63325718f, 0.63389208f, 0.63452639f, 0.63516010f, 0.63579322f,
    0.63642576f, 0.63705770f, 0.63768905f, 0.63831981f, 0.63894998f, 0.63957956f, 0.64020855f, 0.64083696f, 0.64146477f,
    0.64209200f, 0.64271864f, 0.64334469f, 0.64397015f, 0.64459503f, 0.64521932f, 0.64584302f, 0.64646614f, 0.64708868f,
    0.64771063f, 0.64833199f, 0.64895277f, 0.64957296f, 0.65019257f, 0.65081160f, 0.65143005f, 0.65204791f, 0.65266519f,
    0.65328189f, 0.65389800f, 0.65451354f, 0.65512849f, 0.65574287f, 0.65635666f, 0.65696987f, 0.65758251f, 0.65819456f,
    0.65880604f, 0.65941693f, 0.66002725f, 0.66063700f, 0.66124616f, 0.66185475f, 0.66246276f, 0.66307019f, 0.66367705f,
    0.66428334f, 0.66488904f, 0.66549418f, 0.66609874f, 0.66670272f, 0.66730613f, 0.66790897f, 0.66851124f, 0.66911293f,
    0.66971405f, 0.67031460f, 0.67091458f, 0.67151399f, 0.67211283f, 0.67271109f, 0.67330879f, 0.67390592f, 0.67450248f,
    0.67509847f, 0.67569389f, 0.67628874f, 0.67688303f, 0.67747675f, 0.67806990f, 0.67866249f, 0.67925451f, 0.67984597f,
    0.68043686f, 0.68102718f, 0.68161695f, 0.68220614f, 0.68279478f, 0.68338285f, 0.68397036f, 0.68455730f, 0.68514369f,
    0.68572951f, 0.68631477f, 0.68689947f, 0.68748362f, 0.68806720f, 0.68865022f, 0.68923268f, 0.68981459f, 0.69039593f,
    0.69097672f, 0.69155695f, 0.69213662f, 0.69271574f, 0.69329430f, 0.69387231f, 0.69444976f, 0.69502665f, 0.69560300f,
    0.69617878f, 0.69675402f, 0.69732870f, 0.69790282f, 0.69847640f, 0.69904942f, 0.69962189f, 0.70019381f, 0.70076518f,
    0.70133600f, 0.70190627f, 0.70247599f, 0.70304516f, 0.70361378f, 0.70418186f, 0.70474938f, 0.70531636f, 0.70588279f,
    0.70644868f, 0.70701402f, 0.70757881f, 0.70814306f, 0.70870677f, 0.70926993f, 0.70983254f, 0.71039462f, 0.71095615f,
    0.71151713f, 0.71207758f, 0.71263748f, 0.71319684f, 0.71375567f, 0.71431395f, 0.71487169f, 0.71542889f, 0.71598555f,
    0.71654167f, 0.71709726f, 0.71765231f, 0.71820682f, 0.71876079f, 0.71931423f, 0.71986713f, 0.72041949f, 0.72097132f,
    0.72152262f, 0.72207338f, 0.72262361f, 0.72317330f, 0.72372246f, 0.72427109f, 0.72481919f, 0.72536675f, 0.72591379f,
    0.72646029f, 0.72700626f, 0.72755171f, 0.72809662f, 0.72864101f, 0.72918486f, 0.72972819f, 0.73027099f, 0.73081326f,
    0.73135501f, 0.73189623f, 0.73243693f, 0.73297710f, 0.73351674f, 0.73405586f, 0.73459446f, 0.73513253f, 0.73567008f,
    0.73620711f, 0.73674361f, 0.73727960f, 0.73781506f, 0.73835000f, 0.73888442f, 0.73941832f, 0.73995171f, 0.74048457f,
    0.74101691f, 0.74154874f, 0.74208005f, 0.74261084f, 0.74314111f, 0.74367087f, 0.74420011f, 0.74472884f, 0.74525705f,
    0.74578475f, 0.74631193f, 0.74683861f, 0.74736476f, 0.74789041f, 0.74841554f, 0.74894016f, 0.74946427f, 0.74998787f,
    0.75051096f, 0.75103354f, 0.75155561f, 0.75207717f, 0.75259822f, 0.75311876f, 0.75363880f, 0.75415833f, 0.75467735f,
    0.75519587f, 0.75571388f, 0.75623139f, 0.75674839f, 0.75726488f, 0.75778088f, 0.75829637f, 0.75881135f, 0.75932584f,
    0.75983982f, 0.76035330f, 0.76086628f, 0.76137876f, 0.76189074f, 0.76240222f, 0.76291320f, 0.76342368f, 0.76393366f,
    0.76444315f, 0.76495214f, 0.76546063f, 0.76596862f, 0.76647612f, 0.76698312f, 0.76748963f, 0.76799564f, 0.76850116f,
    0.76900619f, 0.76951072f, 0.77001476f, 0.77051831f, 0.77102137f, 0.77152393f, 0.77202600f, 0.77252759f, 0.77302868f,
    0.77352928f, 0.77402940f, 0.77452903f, 0.77502816f, 0.77552682f, 0.77602498f, 0.77652266f, 0.77701985f, 0.77751655f,
    0.77801277f, 0.77850851f, 0.77900376f, 0.77949853f, 0.77999281f, 0.78048661f, 0.78097993f, 0.78147277f, 0.78196512f,
    0.78245700f, 0.78294839f, 0.78343930f, 0.78392974f, 0.78441969f, 0.78490917f, 0.78539816f};
#endif
/*================================================================�ֽ���======================================================================*/
/**************�����������****************/
#if (ATAN_LINER_INTER_EN == 1)
// ʹ�ò�������㷴����ֵ
float _atan(float x)
{
    float   abs_x, frac, result;
    int8_t  sign;
    int16_t index;

    abs_x = _abs(x);
    sign = _sign(x);

    // ���|x|>1,ʹ��atan(x)=pi/2-atan(1/x)
    if (abs_x > 1.0f)
    {
        return sign * (_PI_D2 - _atan(1.0f / abs_x));
    }

    // ��[0,1]��Χӳ�䵽���ұ�����
    index = (int16_t)(abs_x * (ATAN_TABLE_SIZE - 1));

    // ���Բ�ֵ��߾���
    frac = abs_x * (ATAN_TABLE_SIZE - 1) - index;

    if (index < ATAN_TABLE_SIZE - 1)
    {
        result = atan_table[index] + frac * (atan_table[index + 1] - atan_table[index]);
    }
    else
    {
        result = atan_table[index];
    }

    return sign * result;
}
#else
/**************ʹ�ò������������ֵ�����������Բ�ֵ****************/
// ʹ�ò�������㷴����ֵ
float _atan(float x)
{
    float   abs_x, result;
    int8_t  sign;
    int16_t index;

    abs_x = _abs(x);
    sign = _sign(x);

    // ���|x|>1,ʹ��atan(x)=pi/2-atan(1/x)
    if (abs_x > 1.0f)
    {
        return sign * (_PI_D2 - _atan(1.0f / abs_x));
    }

    // ��[0,1]��Χӳ�䵽���ұ�����
    index = (int16_t)(abs_x * (ATAN_TABLE_SIZE - 1));

    // ֱ��ʹ�ò��ұ��е�ֵ
    result = atan_table[index];

    return sign * result;
}
#endif

#endif

// ����atan2
float _atan2(float y, float x)
{
    float angle;
    // �����������
    if (x == 0.0f)
    {
        if (y > 0.0f)
            return _PI_D2;
        if (y < 0.0f)
            return -_PI_D2;
        return 0.0f; // y = 0 ʱ����0
    }

    angle = _atan(y / x);

    // �������޵����Ƕ�
    if (x < 0.0f)
    {
        if (y >= 0.0f)
            angle += _PI;
        else
            angle -= _PI;
    }

    return angle;
}

// 精度只在0.001左右，速度一般,大概280ns 优化等级O3 ，仅供参考学习
// sqrtf精度最高，速度最快 100ns 优化等级O3
float FastSqrt(float number)
{
    float   x = number;
    float   xhalf = 0.5f * x;
    int32_t i = *(int32_t *)&x; // get bits for floating value
    // i = 0x5f3759df - (i >> 1);      // gives initial guess y0
    i = 0x5F34B474 - (i >> 1); // gives initial guess y0

    x = *(float *)&i; // convert bits back to float

    return 1.0f / (x * (1.5f - xhalf * x * x)); // Newton step, repeating increases accuracy
}
float FastInvSqrt(float number)
{
    float   x = number;
    float   xhalf = 0.5f * x;
    int32_t i = *(int32_t *)&x; // get bits for floating value
    // i = 0x5f3759df - (i >> 1);      // gives initial guess y0
    i = 0x5F34B474 - (i >> 1); // gives initial guess y0

    x = *(float *)&i; // convert bits back to float

    return x * (1.5f - xhalf * x * x); // Newton step, repeating increases accuracy
}